﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace GraphSpace
{
    class GoalDirectedSearch
    {
            BinaryHeap<DijkstraKnoten> heap;
            Graph<DijkstraKnoten, Kante> G;
            public GoalDirectedSearch(int StartKnoten, int ZielKnoten)
            {
                heap = new BinaryHeap<DijkstraKnoten>();
                //TODO Graph initialsieren
                G = new Graph<DijkstraKnoten, Kante>();
                int[] punkte = new int[] {0,0,0,1,0,2,0,3,1,3,1,2,1,1,1,0};
                for (int i = 0,j = 0; i < 8; i++,j+=2)
                {
                    if (!G.enthaeltKnoten(i))
                    {
                        G.V.Add(new DijkstraKnoten(i, int.MaxValue, null, 0.0,punkte[j], punkte[j + 1]));				// Knoten aufnehmen
                        G.E.Add(i, new List<Kante>());				// Adjazensliste für Knoten anlegen (ungerichtet)
                    }
                }
                G.neueKante(0, 1, 1);
                G.neueKante(1, 2, 1);
                G.neueKante(2, 3, 1);
                G.neueKante(3, 4, 1);
                G.neueKante(4, 5, 1);
                G.neueKante(5, 6, 1);
                G.neueKante(6, 7, 1);
                G.neueKante(0, 7, 1);
                G.neueKante(1, 6, 1);
                G.neueKante(1, 7, 1);
                G.neueKante(2, 6, 1);
                G.neueKante(2, 5, 1);
                G.neueKante(3, 5, 1);


                // Graph geladen Knoten Distanzen auf oo und Vorgänger auf null            
                DijkstraKnoten initialKnoten = (DijkstraKnoten)G.holeKnoten(StartKnoten);
                initialKnoten.distance = 0;
                DijkstraKnoten zielKnoten = (DijkstraKnoten)G.holeKnoten(ZielKnoten);

                DijkstraKnoten[] path;
                double distance = search(G, initialKnoten, zielKnoten, out path);

                for (int i = 0; i < path.Length; i++)
                {
                    Console.Write(path[i].Id + ", ");
                }
                Console.WriteLine();
                Console.WriteLine(distance);
                Console.ReadLine();
            }


            private double search(Graph<DijkstraKnoten, Kante> G, DijkstraKnoten initialKnoten, DijkstraKnoten zielKnoten, out DijkstraKnoten[] path)
            {
                for (int i = 0; i < G.V.Count; i++)
                {
                    heap.insert((DijkstraKnoten)G.V[i]);
                }

                while (!heap.isEmpty())
                {
                    DijkstraKnoten aktKnoten = heap.extractHead();
                    if (aktKnoten.Id == zielKnoten.Id)
                        break;
                    aktKnoten.besucht = true;
                    for (int i = 0; i < G.E[aktKnoten.Id].Count; i++)
                    {
                        DijkstraKnoten ZielKnoten = (DijkstraKnoten)G.E[aktKnoten.Id][i].Vnach;
                        if (!ZielKnoten.besucht)
                        {
                            // neue abstandsberechnung fuer AStar
                            //int NewDist = aktKnoten.distance + (int)G.E[aktKnoten.Id][i].Gewicht;
                            int AbstandX =  zielKnoten.x - ((DijkstraKnoten)G.E[aktKnoten.Id][i].Vvon).x;
                            int AbstandY = zielKnoten.y - ((DijkstraKnoten)G.E[aktKnoten.Id][i].Vvon).y;
                            double NewDist = aktKnoten.distance + (int)G.E[aktKnoten.Id][i].Gewicht + (int)Math.Sqrt(AbstandX * AbstandX + AbstandY * AbstandY);                
                            if (NewDist < ((DijkstraKnoten)G.E[aktKnoten.Id][i].Vnach).distance)
                            {
                                ZielKnoten.predeseccor = aktKnoten;
                                heap.decreaseKey(ZielKnoten.getHeapPos(), NewDist);
                                ZielKnoten.realdist = aktKnoten.realdist + G.E[aktKnoten.Id][i].Gewicht;
                            }
                        }
                    }

                }
                DijkstraKnoten pred = zielKnoten;
                List<DijkstraKnoten> Path = new List<DijkstraKnoten>();
                while (pred != null)
                {
                    Path.Add(pred);
                    pred = (DijkstraKnoten)pred.predeseccor;
                }

                Path.Reverse();
                path = Path.ToArray();
                return zielKnoten.realdist;
            }


    }
}
